#include <iostream>
#include "read_data.h"

#include "optimizer.h"

using namespace std;

int main() {  
	double acc_noise_std = 0.1;
	double init_bg = 0.0;
	std::vector<double> timestamps;
	std::vector<Eigen::Vector3d> imu_data;  
	std::vector<Eigen::Vector3d> odometry_data;
	std::vector<Eigen::Vector3d> vel_data;  
	std::vector<Eigen::Vector3d> pose_data;
	std::vector<Eigen::Vector3d> gps_data;
	read_simulate(init_bg, timestamps, imu_data, odometry_data, vel_data, pose_data, gps_data);

	// initial wheel 
	WheelOptions wheel_options;
	wheel_options.sigma_x = 1e-8;
	wheel_options.sigma_y = 1e-8;
	wheel_options.sigma_t = 1e-8;
	std::shared_ptr<WheelPreIntegration> wheel_preintegration = std::make_shared<WheelPreIntegration>(wheel_options);
    Optimizer optimizer(wheel_preintegration);

	// initial state
    State state;  
    memset(&state, 0.0, sizeof(state));
    std::vector<Eigen::Vector3d> pose_gt = pose_data;

    state.timestamp = timestamps[0];
    state.p = pose_gt[0].head<2>();
    state.R = SO2(pose_gt[0][2]);
   	state.type = "s";

    State last_state = state; 

    Eigen::Vector3d last_pose;
	int id = optimizer.AddVertex(state, true);
	int last_id = id; 
	for(unsigned int i = 1; i < timestamps.size(); i++) {

		cout << "\ncurr index ==== : " << i << std::endl;

		double delta_time = timestamps[i]-timestamps[i-1];

		// cout << "delta_time ==== : " << delta_time << std::endl;

		Eigen::Vector3d last_odometry_o = odometry_data[i-1];
		Eigen::Vector3d curr_odometry_o = odometry_data[i];
		Eigen::Vector3d delta_odometry = curr_odometry_o - last_odometry_o;

		std::cout << " last_odometry_o =======: " << last_odometry_o.transpose() << std::endl;
		std::cout << " curr_odometry_o =======: " << curr_odometry_o.transpose() << std::endl;
		std::cout << " delta_odometry =======: " << delta_odometry.transpose() << std::endl;

		Eigen::Vector3d last_pose = pose_data[i-1];
		Eigen::Vector3d curr_pose = pose_data[i];

		SE2 last_odometry = SE2(last_odometry_o[0], last_odometry_o[1], last_odometry_o[2]);
		SE2 current_odometry = SE2(curr_odometry_o[0], curr_odometry_o[1], curr_odometry_o[2]);

		wheel_preintegration->Integrate(last_odometry, current_odometry);
		
		State& last_state_t = state;
		wheel_preintegration->Predict(last_state_t);

		state.type = "s";
		id = optimizer.AddVertex(state);

		State last_pose_state;
		last_pose_state.timestamp = timestamps[i-1];
		last_pose_state.p = last_pose.head<2>();
		last_pose_state.R = SO2(last_pose[2]);
  		last_pose_state.type = "p";
		
		State pose_state;
		pose_state.timestamp = timestamps[i];
		pose_state.p = curr_pose.head<2>();
		pose_state.R = SO2(curr_pose[2]);	
    	pose_state.type = "p";

		if(i > 0) {
			Matrix3d info = Matrix3d::Identity();

            State wheel_state;
            wheel_state.timestamp = timestamps[i];
            wheel_state.p = wheel_preintegration->GetTranslation();
            wheel_state.R = wheel_preintegration->GetSO2();
            wheel_state.type = "w";
            info = wheel_preintegration->GetCov().inverse();
            optimizer.AddEdge(PoseGraphEdge(id-1, id, wheel_state, info));

            Matrix3d pose_info = 100 * Matrix3d::Identity();
    		double last_pose_sigma = 1e-15;
    		double curr_pose_sigma = 1e-15;
    		Eigen::Matrix<double, 3, 3> last_pose_covariance = Eigen::Matrix<double, 3, 3>::Identity();
    		last_pose_covariance(0,0) = std::pow(last_pose_sigma,2);
    		last_pose_covariance(1,1) = std::pow(last_pose_sigma,2);
    		last_pose_covariance(2,2) = std::pow(last_pose_sigma,2);
    		Eigen::Matrix<double, 3, 3> last_information_matrix = last_pose_covariance.inverse();

    		Eigen::Matrix<double, 3, 3> curr_pose_covariance = Eigen::Matrix<double, 3, 3>::Identity();
    		curr_pose_covariance(0,0) = std::pow(curr_pose_sigma,2);
    		curr_pose_covariance(1,1) = std::pow(curr_pose_sigma,2);
    		curr_pose_covariance(2,2) = std::pow(curr_pose_sigma,2);
    		Eigen::Matrix<double, 3, 3> curr_information_matrix = curr_pose_covariance.inverse();

    		optimizer.AddEdge(PoseGraphEdge(id-1, id-1, last_pose_state, last_information_matrix));
            optimizer.AddEdge(PoseGraphEdge(id, id, pose_state, curr_information_matrix));

			std::cout << " wheel dp_ =======: " << wheel_preintegration->dp_.transpose() << std::endl;
    		std::cout << " wheel dR_ =======: " << wheel_preintegration->dR_.log() << std::endl;
 
			optimizer.Optimize(20);
			
			optimizer.UpdateState(state, id);

	    	wheel_preintegration = std::make_shared<WheelPreIntegration>(wheel_options);

	    	cout << "After step " << i << ":" << endl;
        	optimizer.PrintPoses();
        	cout << "----------------------" << endl;

			// cout << "curr_odometry ==== : " << curr_odometry_o.transpose() << std::endl;
			// cout << "delta_odometry ==== : " << delta_odometry.transpose() << std::endl;
			// cout << "curr_velocity ==== : " << curr_velocity.transpose() << std::endl;
			cout << "gt_pose ==== : " << curr_pose.transpose() << std::endl;

			// std::cout << "optimize state.R.log() ============= : " << state.R.log() << std::endl;
    		std::cout << "optimize state.pose ============= : " << state.p.transpose() << " " << state.R.log() << std::endl;
    		// std::cout << "optimize state.cov ============= : \n" << state.cov << std::endl;

 			last_id = id;
			last_state = state;
			usleep(150000);		
		}
	}

	return 0;
}
